#include "spl_lio_sam/utility.hpp"
#include "spl_lio_sam/msg/cloud_info.hpp"
#include "spl_lio_sam/srv/save_map.hpp"

class MapSaverClient : public rclcpp::Node
{
public:
    MapSaverClient() : Node("map_saver_client")
    {
        // 创建客户端
        client_ = this->create_client<spl_lio_sam::srv::SaveMap>("/lio_sam/save_map");
    }

    bool send_request(float resolution, const std::string &destination)
    {
        // 等待服务可用
        while (!client_->wait_for_service(std::chrono::seconds(1))) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
                return false;
            }
            RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
        }

        // 创建请求
        auto request = std::make_shared<spl_lio_sam::srv::SaveMap::Request>();
        request->resolution = resolution;
        request->destination = destination;

        // 发送请求并等待响应
        auto result_future = client_->async_send_request(request);

        // 等待响应
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future) ==
            rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_INFO(this->get_logger(), "Map saved successfully to %s with resolution %f", destination.c_str(), resolution);
            return true;
        } else {
            RCLCPP_ERROR(this->get_logger(), "Failed to call service save_map");
            return false;
        }
    }

private:
    rclcpp::Client<spl_lio_sam::srv::SaveMap>::SharedPtr client_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto map_saver_client = std::make_shared<MapSaverClient>();

    // 示例参数
    float resolution = 0.2;
    std::string destination = "/home/cwx/PointCloud_files";

    if (map_saver_client->send_request(resolution, destination)) {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Request sent successfully.");
    } else {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to send request.");
    }

    rclcpp::shutdown();
    return 0;
}